#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

std::string seekerName; 

int main(int argc, char* argv[]){
  ros::init(argc, argv, "seeker_navigation_stim");
  if (argc != 5){ROS_ERROR("Need seeker name, x-coordinate, y-coordinate, and theta to proceed"); return -1;};

  seekerName = argv[1];

  ros::NodeHandle node;
  ros::Publisher posePublisher = node.advertise<geometry_msgs::PoseStamped>("seeker_pose", 2);

  tf::TransformBroadcaster br;
  tf::Pose pose;
  pose.setOrigin( tf::Vector3(atof(argv[2]), atof(argv[3]), 0.0) );
  pose.setRotation( tf::createQuaternionFromYaw(atof(argv[4])) );
  
  ros::Rate rate(30);
  while(node.ok())
  {
    br.sendTransform( tf::StampedTransform(pose, ros::Time::now(), "world", seekerName) );

    tf::Stamped<tf::Pose> stampedPose(pose, ros::Time::now(), "world");
    geometry_msgs::PoseStamped outPose;
    tf::poseStampedTFToMsg(stampedPose, outPose);
    posePublisher.publish(outPose);
   
    rate.sleep();
  }

  ros::spin();
  return 0;
};

